#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <nav_msgs/Path.h>
#include <tf2/utils.h>
#include <mc_core/utils/graphis_utils.h>


class GraphisUtilsTest{
    public:
        GraphisUtilsTest();
        ~GraphisUtilsTest();

        // 测试点到直线距离
        void testDistancePointToLine();

        // 测试圆与直线交点
        void testCircleLineIntersection();

    private:
        ros::NodeHandle nh_;
        ros::Publisher pubPoses_;
};

GraphisUtilsTest::GraphisUtilsTest(){
    pubPoses_ = nh_.advertise<geometry_msgs::PoseArray>("/test_poses", 10);

    testDistancePointToLine();

    testCircleLineIntersection();
}

GraphisUtilsTest::~GraphisUtilsTest(){

}

// 测试点到直线距离
void GraphisUtilsTest::testDistancePointToLine(){
    double distance;
    
    pair<double, double> p = {1.0, 2.0};

    // y轴测试
    pair<double, double> p1 = {0, 1.0};
    pair<double, double> p2 = {0, 3.0};
    distance = graphis_utils::distancePointToLine(p, p1, p2);
    ROS_INFO("[testDistancePointToLine 1] distance: %0.3f <-> 1.0", distance);

    // x轴测试
    p1 = {1.0, 0.0};
    p2 = {3.0, 0.0};
    distance = graphis_utils::distancePointToLine(p, p1, p2);
    ROS_INFO("[testDistancePointToLine 2] distance: %0.3f <-> 2.0", distance);

    // 斜线测试
    p1 = {0.0, 0.0};
    p2 = {3.0, 4.0};
    distance = graphis_utils::distancePointToLine(p, p1, p2);
    ROS_INFO("[testDistancePointToLine 3] distance: %0.3f <-> 0.4", distance);
}

// 测试圆与直线交点
void GraphisUtilsTest::testCircleLineIntersection(){;
    vector<pair<double, double>> intersections;

    pair<double, double> p1 = {-2.0, 0.0};
    pair<double, double> p2 = {2.0, 0.0};
    pair<double, double> c = {0.0, 0.0};
    double radius = 2.0;

    intersections = graphis_utils::circleLineIntersection(p1, p2, c, radius);
    ROS_INFO("[testCircleLineIntersection 1] intersections size: %ld <-> 2", intersections.size());
    for(auto &p : intersections){
        ROS_INFO("[testCircleLineIntersection 1] --> intersection: (%0.3f, %0.3f)", p.first, p.second);
    }

    c = {0.0, 2.0};
    intersections = graphis_utils::circleLineIntersection(p1, p2, c, radius);
    ROS_INFO("[testCircleLineIntersection 2] intersections size: %ld <-> 1", intersections.size());
    for(auto &p : intersections){
        ROS_INFO("[testCircleLineIntersection 2] --> intersection: (%0.3f, %0.3f)", p.first, p.second);
    }

    c = {0.0, 1.0};
    intersections = graphis_utils::circleLineIntersection(p1, p2, c, radius);
    ROS_INFO("[testCircleLineIntersection 3] intersections size: %ld <-> 2", intersections.size());
    for(auto &p : intersections){
        ROS_INFO("[testCircleLineIntersection 3] --> intersection: (%0.3f, %0.3f)", p.first, p.second);
    }
}

int main(int argc, char** argv){
    ros::init(argc, argv, "test_graphis_utils");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
    ROS_INFO("test_graphis_utils start...");

    GraphisUtilsTest testGraphisUtils;
    ros::spin();
    return 0;
}